using System;
using System.Collections.Generic;
using System.Data.Linq;
using System.Text;

namespace GraphSpace
{
	public class BiSearch<PQ1, PQ2> where PQ1 : IPQ<BiDijkstraKnoten>, new() where PQ2 : IPQ<BiDijkstraKnoten>, new()
	{
		private PQ1 PriorityStuct1 = new PQ1 ();
		private PQ2 PriorityStuct2 = new PQ2 ();

		public BiSearch ()
		{
		}

		public double DoSearch (Graph<Knoten, Kante> G, Knoten initialKnoten, Knoten endKnoten, out Knoten[] Path, bool GoalDirected)
		{
			
			for (int i = 0; i < G.V.Count; i++) 
			{
				try 
				{
					PriorityStuct1.insert ((BiDijkstraKnoten)G.V[i]);
					PriorityStuct2.insert ((BiDijkstraKnoten)G.V[i]);
				} 
				catch (Exception e) 
				{
					Console.WriteLine (e.Message);
					Path = new Knoten[0];
					return 0.0;
				}
			}
			try {
				((DijkstraKnoten)initialKnoten).distance = 0;
				while (!PriorityStuct1.isEmpty ()) {
					
					Knoten aktKnoten = PriorityStuct1.extractMin ();
					if (aktKnoten.Id == endKnoten.Id)
						break;
					((DijkstraKnoten)aktKnoten).besucht = true;
					for (int i = 0; i < G.E[aktKnoten.Id].Count; i++) {
						Knoten ZielKnoten = (Knoten)G.E[aktKnoten.Id][i].Vnach;
						if (!((DijkstraKnoten)ZielKnoten).besucht) {
							//Unterschied zwischen normal oder zielgerichtet
							if (GoalDirected) {
								// neue abstandsberechnung fuer AStar
								//int NewDist = aktKnoten.distance + (int)G.E[aktKnoten.Id][i].Gewicht;
								int AbstandX = ((AStarNode)endKnoten).x - ((AStarNode)G.E[aktKnoten.Id][i].Vvon).x;
								int AbstandY = ((AStarNode)endKnoten).y - ((AStarNode)G.E[aktKnoten.Id][i].Vvon).y;
								double NewDist = ((DijkstraKnoten)aktKnoten).distance + G.E[aktKnoten.Id][i].Gewicht + Math.Sqrt (AbstandX * AbstandX + AbstandY * AbstandY);
								if (NewDist < ((AStarNode)G.E[aktKnoten.Id][i].Vnach).distance) {
									((DijkstraKnoten)ZielKnoten).predeseccor = aktKnoten;
									PriorityStuct1.decreaseKey (((DijkstraKnoten)ZielKnoten).getHeapPos (), NewDist);
									((AStarNode)ZielKnoten).realdist = ((AStarNode)aktKnoten).realdist + G.E[aktKnoten.Id][i].Gewicht;
								}
							} else {
								if (!((DijkstraKnoten)ZielKnoten).besucht) {
									double NewDist = ((DijkstraKnoten)aktKnoten).distance + (int)G.E[aktKnoten.Id][i].Gewicht;
									if (NewDist < ((DijkstraKnoten)G.E[aktKnoten.Id][i].Vnach).distance) {
										((DijkstraKnoten)ZielKnoten).predeseccor = aktKnoten;
										PriorityStuct1.decreaseKey (((DijkstraKnoten)ZielKnoten).getHeapPos (), NewDist);
									}
								}
							}
						}
					}
					
				}
				
				if (GoalDirected) {
					AStarNode pred = ((AStarNode)endKnoten);
					List<AStarNode> path = new List<AStarNode> ();
					while (pred != null) {
						path.Add (pred);
						pred = (AStarNode)pred.predeseccor;
					}
					
					path.Reverse ();
					Path = path.ToArray ();
					return ((AStarNode)endKnoten).realdist;
				} else {
					DijkstraKnoten pred = (DijkstraKnoten)endKnoten;
					List<DijkstraKnoten> path = new List<DijkstraKnoten> ();
					while (pred != null) {
						path.Add (pred);
						pred = (DijkstraKnoten)pred.predeseccor;
					}
					
					path.Reverse ();
					Path = path.ToArray ();
					return ((DijkstraKnoten)endKnoten).distance;
				}
			} catch (Exception e) {
				Console.WriteLine (e.Message);
				
			}
			Path = new Knoten[0];
			return 0.0;
		}
	}
}

